#include "NewtonsCradle.h"

#include <NxMath.h>
#include <NxQuat.h>
#include <NxActorDesc.h>
#include <NxBoxShape.h>
#include <NxBoxShapeDesc.h>
#include <NxSphereShape.h>
#include <NxSphereShapeDesc.h>
#include <NxCapsuleShapeDesc.h>

#include <NxDistanceJoint.h>
#include <NxDistanceJointDesc.h>
#include <NxRevoluteJoint.h>
#include <NxRevoluteJointDesc.h>

#include "PhysxLogic.h"

#include <iostream>

NewtonsCradle::NewtonsCradle(const NxVec3 &pos, const NxVec3 &size, NxScene *scene, int numBalls, float ballRadius, float gapBetweenBalls)
{
	this->position = pos;
	this->size = size;
	this->numBalls = numBalls;
	this->ballRadius = ballRadius;

	this->leftFrame = NULL;
	this->rightFrame = NULL;
	this->balls = new NxActor*[numBalls];

	float thicknessFrame = 1.0f;
	float border = (size.z - (2*ballRadius*numBalls) - (numBalls-1)*gapBetweenBalls)/2;
	float ballHeight = (size.y - thicknessFrame/2) - (2.0f/3.0f)*(size.y - ballRadius - thicknessFrame/2);

	//creating frame actors
	NxBoxShapeDesc boxShapeDesc;
	boxShapeDesc.dimensions = NxVec3(thicknessFrame/2, thicknessFrame/2, size.z/2);

	NxActorDesc leftActorFrame;
	leftActorFrame.shapes.pushBack(&boxShapeDesc);
	leftActorFrame.body = NULL; //static actor
	leftActorFrame.globalPose.t = NxVec3(pos.x - size.x/2 + thicknessFrame/2, pos.y + size.y - thicknessFrame/2, pos.z);
	leftActorFrame.userData = this;
	leftFrame = scene->createActor(leftActorFrame);
	leftFrame->raiseActorFlag(NX_AF_DISABLE_COLLISION);
	
	NxActorDesc rightActorFrame;
	rightActorFrame.shapes.pushBack(&boxShapeDesc);
	rightActorFrame.body = NULL; //static actor
	rightActorFrame.globalPose.t = NxVec3(pos.x + size.x/2 - thicknessFrame/2, pos.y + size.y - thicknessFrame/2, pos.z);
	rightActorFrame.userData = this;
	rightFrame = scene->createActor(rightActorFrame);
	rightFrame->raiseActorFlag(NX_AF_DISABLE_COLLISION);

	float k = ((size.x - thicknessFrame)/2.0f);
	float h = (size.y - thicknessFrame/2 - ballHeight);
	float stickAngle = NxMath::atan(k / h);
	float stickRadius = 0.1f;
	float w = ((1/NxMath::sin(stickAngle))*k);
	float stickHeight = w - 2.0f * stickRadius;
	stickAngle = NxMath::radToDeg(stickAngle); //now in degrees

	//
	//creating balls actors
	//
	NxSphereShapeDesc ballShapeDesc;
	ballShapeDesc.radius = ballRadius;
	ballShapeDesc.localPose.t.zero();
    ballShapeDesc.skinWidth = 0.1f;

	NxBodyDesc ballBodyDesc;
	ballBodyDesc.mass = 100.0f;
	ballBodyDesc.massLocalPose.t = NxVec3(0.0f, 0.0f, 0.0f);
	ballBodyDesc.massLocalPose.M.zero();
	ballBodyDesc.angularDamping	= 0.0f;
	ballBodyDesc.linearDamping	= 0.0f;

	//sticks
	NxCapsuleShapeDesc stickLeftDesc;
	stickLeftDesc.radius = stickRadius;
	stickLeftDesc.height = stickHeight;
	NxVec3 stickLeftPosition = NxVec3(-k/2, h/2, 0.0f);
	stickLeftDesc.localPose.t = stickLeftPosition;
	stickLeftDesc.localPose.M = PhysxLogic::CalcOrientationMatrix(NxVec3(0.0f, 0.0f, stickAngle));

	NxCapsuleShapeDesc stickRightDesc;
	stickRightDesc.radius = stickRadius;
	stickRightDesc.height = stickHeight;
	NxVec3 stickRightPosition = NxVec3(k/2, h/2, 0.0f);
	stickRightDesc.localPose.t = stickRightPosition;
	stickRightDesc.localPose.M = PhysxLogic::CalcOrientationMatrix(NxVec3(0.0f, 0.0f, -stickAngle));

	float startZpos = pos.z + size.z/2 - border;
	for(int i = 0; i < numBalls; i++)
	{
		NxActorDesc ballActorDesc;
		ballActorDesc.shapes.pushBack(&ballShapeDesc);
		ballActorDesc.shapes.pushBack(&stickLeftDesc);
		ballActorDesc.shapes.pushBack(&stickRightDesc);
		ballActorDesc.body = &ballBodyDesc;
		ballActorDesc.globalPose.t = NxVec3(pos.x,  pos.y + ballHeight, startZpos - ballRadius);
		ballActorDesc.userData = this;
		balls[i] = scene->createActor(ballActorDesc);

		startZpos -= 2*ballRadius + gapBetweenBalls;
	}

	//
	//creating joints
	//

	//joint limits
	NxJointLimitDesc lowLimit;
	lowLimit.value = -NxPiF32/2;
	NxJointLimitDesc highLimit;
	highLimit.value = NxPiF32/2;

	NxJointLimitPairDesc jointLimit;
	jointLimit.low = lowLimit;
	jointLimit.high = highLimit;

	for(int i = 0; i < numBalls; i++)
	{
		NxActor *ball = balls[i];

		NxRevoluteJointDesc revJDesc;
		revJDesc.flags |= NX_RJF_LIMIT_ENABLED;
		revJDesc.actor[0] = NULL; //world space, because statics are not allowed
		revJDesc.actor[1] = ball;
		revJDesc.setGlobalAnchor(NxVec3(ball->getGlobalPosition().x, leftFrame->getGlobalPosition().y, ball->getGlobalPosition().z));
		revJDesc.setGlobalAxis(NxVec3(1.0f, 0.0f, 0.0f));
		revJDesc.limit = jointLimit;

		scene->createJoint(revJDesc);
	}
}

NewtonsCradle::~NewtonsCradle()
{
	if(this->balls != NULL)
	{
		for(int i = 0; i < numBalls; ++i)
		{
			PhysxLogic::physicsScene->releaseActor(*balls[i]);
		}
		delete[] balls;
		balls = NULL;
	}
	if(this->rightFrame != NULL)
	{
		PhysxLogic::physicsScene->releaseActor(*(this->rightFrame));
		this->rightFrame = NULL;
	}
	if(this->leftFrame != NULL)
	{
		PhysxLogic::physicsScene->releaseActor(*(this->leftFrame));
		this->leftFrame = NULL;
	}
}

void NewtonsCradle::Draw()
{
	float matrix[16];
    const static float shadowMatrix[] = { 1,0,0,0, 0,0.001f,0,0, 0,0,1,0, 0,0,0,1 };
	//balls
	for(int i = 0; i < numBalls; ++i)
	{
		NxActor *ball = balls[i];		
		ball->getGlobalPose().getColumnMajor44(matrix);

		//ball
		glPushMatrix();
		glMultMatrixf(matrix);
		glDisable(GL_COLOR_MATERIAL);
		float ambient[] = { 0.45f, 0.45f, 0.45f, 1.0f };
		float diffuse[] = { 0.5f, 0.5f, 0.5f, 1.0f };
		float specular[] = { 0.77f, 0.77f, 0.77f, 1.0f };
		float shininess = 50.2f;
		glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, ambient);
		glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, diffuse);
		glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, specular);
		glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, shininess);
		glutSolidSphere(this->ballRadius, 64, 64);
		glPopMatrix();

        //ball shadow
        glDisable(GL_LIGHTING);
        glPushMatrix();
        glMultMatrixf(shadowMatrix);
        glMultMatrixf(matrix);
        glColor4f(0.0f, 0.0f, 0.0f, 0.4f);
        glutSolidSphere(this->ballRadius, 64, 64);
        glPopMatrix();
        glEnable(GL_LIGHTING);
		

		//sticks
		for(unsigned int k = 0; k < ball->getNbShapes(); ++k)
		{
			NxShape *shape = ball->getShapes()[k];
			if(NxCapsuleShape *stick = shape->isCapsule())
			{
				stick->getGlobalPose().getColumnMajor44(matrix);
				glPushMatrix();
				glMultMatrixf(matrix);				
				Renderer::DrawCapsule(stick->getHeight(), stick->getRadius());
				glPopMatrix();

                //shadow
                glDisable(GL_LIGHTING);
                glPushMatrix();
                glMultMatrixf(shadowMatrix);
                glMultMatrixf(matrix);
                glColor4f(0.0f, 0.0f, 0.0f, 0.2f);
                Renderer::DrawCapsule(stick->getHeight(), stick->getRadius());
                glPopMatrix();
                glEnable(GL_LIGHTING);
			}			
		}
		glEnable(GL_COLOR_MATERIAL);

		//frames
		glColor3f(0.5f, 0.35f, 0.25f);
		for(unsigned int k = 0; k < this->rightFrame->getNbShapes(); ++k)
		{
			NxShape *shape = this->rightFrame->getShapes()[k];
			if(NxBoxShape *frame = shape->isBox())
			{
				frame->getGlobalPose().getColumnMajor44(matrix);
				glPushMatrix();
				glMultMatrixf(matrix);				
				Renderer::DrawBox(frame->getDimensions());
				glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
				glLineWidth(1.0f);
				glColor3f(0.0f, 0.0f, 0.0f);
				Renderer::DrawBox(frame->getDimensions());
				glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
				glPopMatrix();

                //shadow
                glDisable(GL_LIGHTING);
                glPushMatrix();
                glMultMatrixf(shadowMatrix);
                glMultMatrixf(matrix);
                glColor4f(0.0f, 0.0f, 0.0f, 0.4f);
                Renderer::DrawBox(frame->getDimensions());
                glPopMatrix();
                glEnable(GL_LIGHTING);
			}
		}
		glColor3f(0.5f, 0.35f, 0.25f);
		for(unsigned int k = 0; k < this->leftFrame->getNbShapes(); ++k)
		{
			NxShape *shape = this->leftFrame->getShapes()[k];
			if(NxBoxShape *frame = shape->isBox())
			{
				frame->getGlobalPose().getColumnMajor44(matrix);
				glPushMatrix();
				glMultMatrixf(matrix);				
				Renderer::DrawBox(frame->getDimensions());
				glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
				glLineWidth(1.0f);
				glColor3f(0.0f, 0.0f, 0.0f);
				Renderer::DrawBox(frame->getDimensions());
				glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
				glPopMatrix();

                //shadow
                glDisable(GL_LIGHTING);
                glPushMatrix();
                glMultMatrixf(shadowMatrix);
                glMultMatrixf(matrix);
                glColor4f(0.0f, 0.0f, 0.0f, 0.4f);
                Renderer::DrawBox(frame->getDimensions());
                glPopMatrix();
                glEnable(GL_LIGHTING);
			}
		}		
	}
}